#include "vanjee_mini_lidar/vanjee_716mini_node.h"
#include "pugixml.hpp"
#include <boost/program_options.hpp>
#include <fstream>
#include <spdlog/spdlog.h>
#include <regex>

namespace po = boost::program_options;
namespace vanjee_lidar
{
    Vanjee716MiniNode::Vanjee716MiniNode(int argc, char **argv)
    {

        po::options_description desc("Allowed options");
        desc.add_options()("help,h", "produce help message");
        desc.add_options()("default,d", "use default option values");
        desc.add_options()("lidar_ip,f", po::value<std::string>()->default_value("192.168.0.2"), "LiDAR IP");
        desc.add_options()("lidar_port,F", po::value<int>()->default_value(2110), "LiDAR port");
        desc.add_options()("simple_port,T", po::value<int>()->default_value(4242), "port to which messages in simple protocol are sent");
        desc.add_options()("scan_gear", po::value<int>()->default_value(2), "1->15Hz, 2->25Hz");
        desc.add_options()("communication_mode,m", po::value<int>()->default_value(0), "0:tcp; 1:udp");
        desc.add_options()("host_ip", po::value<std::string>()->default_value("192.168.150.131"), "UDP host IP");
        desc.add_options()("host_port", po::value<int>()->default_value(6060), "UDP host port");
        desc.add_options()("log_level,l", po::value<std::string>()->default_value("info"),
                           "Available log levels in SPDLOG include (in ascending order of severity): trace, debug, info, warn, err, critical, and off.");
        // desc.add_options()("config_file,c", po::value<std::string>()->default_value("config.xml"), "full path to configuration file");

        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, desc), vm);
        po::notify(vm);

        if (argc < 2 || vm.count("help"))
        {
            std::cout << "Usage: " << argv[0] << " [options]\n";
            std::cout << desc << "\n";
            std::cout << "Example 1: To transfer laser data from LiDAR with address 192.168.8.150 and port 5555 to port 9090\n\t"
                      << argv[0] << " -f 192.168.8.150 -F 5555 -T 9090\n";
            std::cout << "Example 2: To transfer laser data from LiDAR with default address 192.168.0.2 and default port 2110 to port 9090\n\t"
                      << argv[0] << " -T 9090\n";
            std::cout << "Example 3: To transfer laser data with all default option values, from LiDAR 192.168.0.2:2110 to port 4242\n\t"
                      << argv[0] << " -d\n";
            // return 0;
            std::exit(0);
        }

        if (vm.count("default"))
        {
            lidar_ip_ = vm["lidar_ip"].as<std::string>();
            lidar_port_ = vm["lidar_port"].as<int>();
            simple_port_ = vm["simple_port"].as<int>();
            scan_gear_ = vm["scan_gear"].as<int>();
            communication_mode_ = vm["communication_mode"].as<int>();
            host_ip_ = vm["host_ip"].as<std::string>();
            host_port_ = vm["host_port"].as<int>();
            log_level_ = vm["log_level"].as<std::string>();
            // config_file = vm["config_file"].as<std::string>();
        }
        else
        {
            if (vm.count("lidar_ip"))
            {
                if (not validateIPAddress(lidar_ip_ = vm["lidar_ip"].as<std::string>()))
                {
                    std::cout << "Invalid IP address format of LiDAR: " << lidar_ip_ << std::endl;
                    // return 1;
                    std::exit(1);
                }
            }

            if (vm.count("lidar_port"))
            {
                lidar_port_ = vm["lidar_port"].as<int>();
            }

            if (vm.count("simple_port"))
            {
                simple_port_ = vm["simple_port"].as<int>();
            }

            if (vm.count("scan_gear"))
            {
                scan_gear_ = vm["scan_gear"].as<int>();
            }

            if (vm.count("communication_mode"))
            {
                communication_mode_ = vm["communication_mode"].as<int>();
            }

            if (vm.count("host_ip"))
            {
                if (not validateIPAddress(host_ip_ = vm["host_ip"].as<std::string>()))
                {
                    std::cerr << "Invalid IP address format of LiDAR: " << host_ip_ << std::endl;
                    std::exit(1);
                }
            }

            if (vm.count("host_port"))
            {
                host_port_ = vm["host_port"].as<int>();
            }

            if (vm.count("log_level"))
            {
                log_level_ = vm["log_level"].as<std::string>();
            }

            // if (vm.count("config_file"))
            // {
            //     config_file_ = vm["config_file"].as<std::string>();
            // }
        }

        // TODO
        communication_mode_ = vm["communication_mode"].as<int>();
        host_ip_ = vm["host_ip"].as<std::string>();
        host_port_ = vm["host_port"].as<int>();

        // TODO use all parameters
        
        // Set global log level
        std::map<std::string, spdlog::level::level_enum> log_levels = {
            {"trace", spdlog::level::trace},
            {"debug", spdlog::level::debug},
            {"info", spdlog::level::info},
            {"warn", spdlog::level::warn},
            {"err", spdlog::level::err},
            {"critical", spdlog::level::critical},
            {"off", spdlog::level::off}};
        auto it = log_levels.find(log_level_);
        if (it != log_levels.end())
        {
            spdlog::set_level(it->second);
        }
        else
        {
            throw std::runtime_error(fmt::format("Invalid log level: {}", log_level_));
        }

        // configure spdlog to display log entries with only the message content and
        // nothing else (no timestamps, log levels, thread IDs, etc.).
        spdlog::set_pattern("%v");

        spdlog::info(R"(
Arguments:
LiDAR IP: {}
LiDAR port: {}
simple port: {}
scan gear: {}
scan frequency: {}
communication mode: {}
log level: {}
)",

                     lidar_ip_,
                     lidar_port_,
                     simple_port_,
                     scan_gear_,
                     (scan_gear_ == 2 ? "25Hz" : "15Hz"),
                     (communication_mode_ == 0 ? "TCP" : "UDP"),
                     log_level_);

        lidar_protocol_ = new Vanjee716MiniLidarProtocol(scan_gear_, simple_port_);
        // dynamic_reconfigure::Server<vanjee_mini_lidar::vanjee_716mini_lidarConfig>::CallbackType f;
        // f = boost::bind(&Vanjee716MiniNode::callback, this, _1, _2);
        // server_.setCallback(f);

        connect();
    }

    Vanjee716MiniNode::~Vanjee716MiniNode()
    {
        if (commumication_)
        {
            delete commumication_;
        }
        delete lidar_protocol_;
    }

    // void Vanjee716MiniNode::callback(vanjee_mini_lidar::vanjee_716mini_lidarConfig &config, uint32_t level)
    // {
    //     lidar_protocol_->setConfig(config, level);
    // }

    void Vanjee716MiniNode::connect()
    {
        if (communication_mode_ == 0) // tcp
        {
            spdlog::info("communication_mode: TCP");
            commumication_ = new VanjeeCommonTcp(lidar_protocol_);
        }
        else if (communication_mode_ == 1) // udp
        {
            spdlog::info("communication_mode: UDP");
            commumication_ = new VanjeeCommonUdp(host_ip_, host_port_, lidar_protocol_);
        }
        while (!commumication_->connected_)
        {
            spdlog::info("Start connecting...");
            if (commumication_->connect(lidar_ip_.c_str(), lidar_port_))
            {
                spdlog::info("Succesfully connected. Hello vanjee_716mini_lidar!");
            }
            else
            {
                spdlog::info("Failed to connect. Waiting 5s to reconnect!");
            }
            // ros::Duration(5).sleep();
            sleep(5);
        }
        lidar_protocol_->heartstate_ = false;
    }

    void Vanjee716MiniNode::checkConnection()
    {
        if (commumication_->connected_)
        {
            if (lidar_protocol_->heartstate_)
            {
                lidar_protocol_->heartstate_ = false;
            }
            else
            {
                commumication_->connected_ = false;
            }
        }
        else
        {
            if (!commumication_->reconnecting_)
            {
                boost::thread t(boost::bind(&VanjeeCommonAbstract::reconnect, commumication_));
            }
        }
    }

    bool Vanjee716MiniNode::validateIPAddress(const std::string &ipAddress)
    {
        // match an IP address in the format of "xxx.xxx.xxx.xxx"
        // const std::regex pattern("^([0-9]{1,3}\\.){3}[0-9]{1,3}$");

        // a stricter regular expression for matching IPv4 addresses
        const std::regex pattern("^((25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.){3}(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$");
        // const std::regex pattern("^(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)\\.(25[0-5]|2[0-4][0-9]|[01]?[0-9][0-9]?)$");
        return std::regex_match(ipAddress, pattern);
    }
}

int main(int argc, char **argv)
{
    vanjee_lidar::Vanjee716MiniNode *vanjee_node = new vanjee_lidar::Vanjee716MiniNode(argc, argv);

    while (true)
    {
        // ros::Duration(2).sleep();
        sleep(2);
        vanjee_node->checkConnection();
        // server.write(buffer.begin(), buffer.size());

        // ros::spinOnce();
    }
    delete vanjee_node;
    return 0;
}
